{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "quantitative-muslim",
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "from matplotlib import pyplot as plt\n",
    "import numpy as np\n",
    "from argparse import Namespace\n",
    "import json\n",
    "import torch"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "amazing-millennium",
   "metadata": {},
   "outputs": [],
   "source": [
    "from scripts.train import DataModule, _to_image, KeypointModule\n",
    "from perception.loss import KeypointLoss"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "protective-myanmar",
   "metadata": {},
   "outputs": [],
   "source": [
    "with open('../config/cups.json', 'rt') as f:\n",
    "    keypoint_config = json.load(f)\n",
    "module = DataModule(Namespace(train='/home/ken/data/cups_train/', val='/home/ken/data/cups_test/', batch_size=2, workers=1, pool=256), keypoint_config=keypoint_config)\n",
    "module.setup('fit')\n",
    "\n",
    "train_loader = module.train_dataloader()\n",
    "train_iterator = iter(train_loader)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "square-college",
   "metadata": {},
   "outputs": [],
   "source": [
    "print(len(train_loader))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "sought-collins",
   "metadata": {},
   "outputs": [],
   "source": [
    "from tqdm import tqdm\n",
    "for _ in tqdm(range(10)):\n",
    "    next(train_iterator)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "private-bloom",
   "metadata": {},
   "outputs": [],
   "source": [
    "import cv2\n",
    "def resize(target, width=640, height=360):\n",
    "    return cv2.resize(target, (width, height), interpolation=cv2.INTER_LINEAR)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "typical-positive",
   "metadata": {},
   "outputs": [],
   "source": [
    "module = KeypointModule(keypoint_config=keypoint_config)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "brilliant-jonathan",
   "metadata": {},
   "outputs": [],
   "source": [
    "model = module.model"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "equipped-caution",
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "frame, target, depth, centers = next(train_iterator)\n",
    "\n",
    "def to_p(target):\n",
    "    return target\n",
    "\n",
    "plt.figure(figsize=(7, 4 * frame.shape[0]))\n",
    "rows = frame.shape[0]\n",
    "for i in range(rows):\n",
    "    axis = plt.subplot2grid((rows, 1), loc=(i, 0))\n",
    "    axis.imshow(_to_image(frame[i].numpy()))\n",
    "    axis.imshow(resize(to_p(target[i]).sum(dim=0).numpy()), alpha=0.7, vmin=0.0, vmax=1.0)\n",
    "    plt.axis('off')\n",
    "plt.tight_layout()\n",
    "pass"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ultimate-northern",
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "optimizer = torch.optim.Adam(lr=3e-4, params=model.parameters())\n",
    "loss_fn = KeypointLoss(keypoint_config['keypoint_config'])\n",
    "model.train()\n",
    "progress = tqdm(range(100))\n",
    "for i in progress:\n",
    "    p_heatmaps, p_depth, p_centers = model(frame)\n",
    "    loss_value, _ = loss_fn(p_heatmaps, target, p_depth, depth, p_centers, centers)\n",
    "    progress.set_postfix({'loss': loss_value.item()})\n",
    "    loss_value.backward()\n",
    "    optimizer.step()\n",
    "    optimizer.zero_grad()\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "possible-banking",
   "metadata": {},
   "outputs": [],
   "source": [
    "from torch.nn import functional as F\n",
    "print(F.binary_cross_entropy_with_logits(p_heatmaps, target, reduction='none').sum(dim=[1,2,3]).mean() * loss_fn.heatmap_weight)\n",
    "print(loss_fn.reduce(F.l1_loss(p_depth[depth > 0.05], depth[depth > 0.05], reduction='none')))\n",
    "print(loss_fn.reduction)\n",
    "print(loss_fn.focal_loss(p_heatmaps, target).max())\n",
    "print(target.max())\n",
    "print(torch.sigmoid(p_heatmaps).max())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ordinary-arthur",
   "metadata": {},
   "outputs": [],
   "source": [
    "with torch.no_grad():\n",
    "    model.eval()\n",
    "    heatmap_p, depth_p, centers_p = model(frame)\n",
    "    heatmap_p = torch.sigmoid(heatmap_p)\n",
    "plt.figure(figsize=(7, 4 * frame.shape[0]))\n",
    "rows = frame.shape[0]\n",
    "for i in range(rows):\n",
    "    axis = plt.subplot2grid((rows, 1), loc=(i, 0))\n",
    "    axis.imshow(_to_image(frame[i].numpy()))\n",
    "    axis.imshow(resize(heatmap_p[i].sum(dim=0).numpy()), alpha=0.7, vmin=0.0, vmax=1.0)\n",
    "    plt.axis('off')\n",
    "pass"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "extreme-pacific",
   "metadata": {},
   "outputs": [],
   "source": [
    "pixel_indices = np.zeros((2, 180, 320), dtype=np.float32)\n",
    "for i in range(180):\n",
    "    for j in range(320):\n",
    "        pixel_indices[:, i, j] = [j + 0.5, i + 0.5]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "favorite-organ",
   "metadata": {},
   "outputs": [],
   "source": [
    "vectors = centers.numpy()[0]\n",
    "norms = np.linalg.norm(vectors, axis=0)\n",
    "where_non_zero = np.abs(norms) > 1e-2\n",
    "gt_centers = np.zeros((2, pixel_indices.shape[1], pixel_indices.shape[2]), dtype=np.float32)\n",
    "gt_centers = pixel_indices + vectors\n",
    "\n",
    "where_heatmap_non_zero = target[0].numpy().sum(axis=0) > 0.1\n",
    "\n",
    "p_centers = np.zeros((2, pixel_indices.shape[1], pixel_indices.shape[2]), dtype=np.float32)\n",
    "p_centers = pixel_indices + centers_p[0].detach().numpy()\n",
    "\n",
    "figure = plt.figure(figsize=(10, 5))\n",
    "dotted_image = cv2.resize(_to_image(frame[0].numpy().copy()), (320, 180))\n",
    "for point in gt_centers[:, where_non_zero].transpose():    \n",
    "    cv2.circle(dotted_image, (point[0], point[1]), 2, (255, 0, 0), -1)\n",
    "    \n",
    "dotted_image_pred = cv2.resize(_to_image(frame[0].numpy().copy()), (320, 180))\n",
    "where_pred_non_zero = heatmap_p[0].sum(dim=0) > 0.25\n",
    "for point in p_centers[:, where_pred_non_zero].transpose():    \n",
    "    cv2.circle(dotted_image_pred, (point[0], point[1]), 2, (255, 0, 0), -1)\n",
    "\n",
    "axis = plt.subplot2grid((1, 2), loc=(0, 0))\n",
    "axis.imshow(dotted_image)\n",
    "plt.axis('off')\n",
    "\n",
    "axis = plt.subplot2grid((1, 2), loc=(0, 1))\n",
    "axis.imshow(dotted_image_pred)\n",
    "plt.axis('off')\n",
    "pass"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "rapid-seating",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "spoken-oxide",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "liked-ensemble",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "vision",
   "language": "python",
   "name": "vision"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
